/*
 * ActuatingLoop.cpp
 *
 *  Created on: Sep 4, 2009
 *      Author: zurihopper
 */

#include "ActuatingLoop.hpp"
#include <ecl/time.hpp>
#include <stdio.h>
#include "./../Common/Common.hpp"
#include <math.h>

using Octopus::Common::sWait;
using Octopus::Common::uWait;

using Octopus::BoardSetup::TitechBoard;
using ECL::Time::StopWatch;
using ECL::Time::TimeStamp;
using Octopus::ToolKit::Log::Logger;
//using ECL::Time::

namespace Octopus {

namespace MainLoop {

//ControlLoop * ControlLoop::controlLoop;

//ActuatingLoop

Loop * ActuatingLoop::getInstance(Logger *log) {
	if(loop == NULL)
	{
		loop = new ActuatingLoop(log);
	}
	return(loop);
}

void ActuatingLoop::remove() {
	printf("Removing Control Loop\n");

	while(!loopFinished)
	{
//		uWait(10000);
		sWait(0.1);
		printf("waiting for thread to finish\n");
	}

	delete(this);
}

void ActuatingLoop::start() {

	int ctr = 1;
	StopWatch stopwatch;
	TimeStamp time;

	long int loopTime = (long int)LOOP_TIME;
	long int time1 = 0, time2 = 0, time3 = 0, timeDiff = 0, totTime = 0, time1Prev= 0;
	double avgTime;

	int lMeasDigital=0;
	int motorOutDigital= 0;
//	int pulseOut = 0;
	double fMeasAnalog = 0.0;//, motorOutAnalog = 0.0;
	//, TsetPoint = 0.0, lastError = 0.0, errorDot = 0.0;

	vector<double>TsetPoint(MAX_NUM_OF_MOTOR, 0.0);
//	vector<double>lastError(MAX_NUM_OF_MOTOR, 0.0);
//	vector<double>errorDot(MAX_NUM_OF_MOTOR, 0.0);
//	vector<double>errorInt(MAX_NUM_OF_MOTOR, 0.0);
//	vector<double>offset(MAX_NUM_OF_MOTOR, 0.0);
//	vector<double>error(MAX_NUM_OF_MOTOR,0.0);
//	vector<bool> isMotorReady(MAX_NUM_OF_MOTOR,false);

	//double errorInt = 0.0;
	//double offset = 0.0;

//	bool initialising = true;

	loopFinished = false;
	//Initialising Loop

	long double timeCtr = 0.0;

	while(true)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");

//		setPointMutex.lock();
//		TsetPoint = setPoints;
//		setPointMutex.unlock();

//		TsetPoint.at(0) = 0.0;
//		TsetPoint.at(1) = 0.0;
//		TsetPoint.at(2) = 0.0;

#if TRAJECTORY_TYPE ==1

		TsetPoint.at(3) = 1160 + (60 * sin((double)WAVE_FREQUENCY * timeCtr));
		TsetPoint.at(0) = 1138 + (38 * sin((double)WAVE_FREQUENCY * timeCtr + ((double)PI)));

#else
		TsetPoint.at(1) = 40 + 20 * sin((double)WAVE_FREQUENCY * tempVal);
		TsetPoint.at(2) = 40 + 20 * sin((double)WAVE_FREQUENCY * tempVal + ((double)PI/2));
#endif


		printf ("%Lf : ", timeCtr);
		timeCtr = timeCtr + 0.05;
		for(int i=0; i<MAX_NUM_OF_MOTOR && (unsigned int)i<temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			lMeasDigital = temp.at(i);
//			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);
			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * (((double)lMeasDigital)/1023);

//			motorOutDigital = (int)(1100 + 50 + 800*(((double)TsetPoint.at(i)*10)/(3.5*360)));
			motorOutDigital = (int) TsetPoint.at(i);

			if(motorOutDigital<1100) {

				motorOutDigital = 1100;
			}

//			pulseOut = (int)(1100 + 800*((double)lMeasDigital/1023));

//			double thetaTemp = ((double)theta/1023);
//			pulseOut = (int)(1100 + 800*((double)heta/1023));
//			pulseOut = (int)(1100+ 800*(offset/1260));


//			double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

//			printf("%d: SP: %f, Act(N): %f, e: %f, eDot: %f, eInt: %f, KPe : %f, KDeDot : %f, KIeInt : %f", i, TsetPoint.at(i), fMeasAnalog, error.at(i), errorDot.at(i), errorInt.at(i), (double)K_P * error.at(i), (double)K_D * errorDot.at(i), (double)K_I * errorInt.at(i));
//			printf("%d: SP: %f, Act(N): %f ", i, (double)TsetPoint.at(i)*10, fMeasAnalog);
			printf("%d: Meas: %f, Act: %d ", i,fMeasAnalog , motorOutDigital);
//			titechBoard->writeMotor(i,pulseOut);
			titechBoard->writeMotor(i,motorOutDigital);

//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
		}
		printf("\n");

//		tempVal++;

		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);

		time = stopwatch.elapsed();

		time3 = time.usec();

		// Job
		totTime += time3 - time1;
		avgTime = (double)totTime / (double)ctr;
		ctr++;

//		printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

		time1Prev = time1;

		stopwatch.restart();

	}


	//Resetting motors

	for(int i=0; i<MAX_NUM_OF_MOTOR ;i++)
	//		for(int i=0; i<temp.size();i++)
	{

		titechBoard->writeMotor(i,1100);
	}


	loopFinished = true;
	printf ("Average LoopTime was %f\n", avgTime);
	printf("ControlLoop thread closing\n");
}

bool ActuatingLoop::stop()
{
	printf("Trying to stop Loop");
	mutex.lock();
	continueLoop = false;
	mutex.unlock();
	return(continueLoop);
}
bool ActuatingLoop::finished()
{
	return(loopFinished);
}

bool ActuatingLoop::changeSetPoints(vector<double> sp) {
	setPointMutex.lock();
	setPoints = sp;
	setPointMutex.unlock();
	return(true);
}

ActuatingLoop::ActuatingLoop(Logger *log) : sensorValues(MAX_NUM_OF_SENSOR,0), motorCommands(MAX_NUM_OF_MOTOR,0), setPoints(MAX_NUM_OF_MOTOR,0.0) {
	logger = log;
	titechBoard = TitechBoard::getInstance();
	continueLoop = true;
	loopFinished = true;
	offsetForce = 0.0;
	printf("Created New ControlLoop\n");
}

ActuatingLoop::~ActuatingLoop() {
	titechBoard->remove();
}

}

}
